/**
 * @file QWF_Math.c
 * @brief 自己的数学库，储存自己累计下来的好用的一些数学函数
 * @author WeiFan Qiu (qiuweifan0523@faxmail.com)
 * @version 1.0
 * @date 2025-08-07
 * 
 * @copyright Copyright (c) 2025  邱伟凡
 * 
 * @par 修改日志:
 * - 2025-08-07: 初始版本，实现快速余弦(fastCos)、正弦(fastSin)、开平方(quakeSqrt)及角度处理(To_0_T/find_min_angle/Helm_angle)函数
 * - 2025-08-08: 补充函数详细注释，分析6项潜在bug并完善文档（邱伟凡）
 * @bug 待解决问题:
 */
#include "QWF_Math.h"
#include "arm_math.h"
#include "math.h"
//把0-90的所有整数角度的cos值存起来，在余弦值精度要求不那么大的时候使用（抄中南大学的思路）
static float fast_cos_buf_[91] = { 1.0f,//0
    0.999848f	,0.999391f	,0.99863f		,0.997564f		,0.996195f	,0.994522f	,0.992546f	,0.990268f	,0.987688f	,0.984808f		,/*1-10*/
	0.981627f	,0.978148f	,0.97437f		,0.970296f		,0.965926f	,0.961262f	,0.956305f	,0.951057f	,0.945519f	,0.939693f		,/*11-20*/
	0.93358f	,0.927184f	,0.920505f		,0.913545f		,0.906308f	,0.898794f	,0.891007f	,0.882948f	,0.87462f	,0.866025f		,/*21-30*/
	0.857167f	,0.848048f	,0.838671f		,0.829038f		,0.819152f	,0.809017f	,0.798635f	,0.788011f	,0.777146f	,0.766044f		,/*31-40*/
    0.754710f	,0.743145f	,0.731354f		,0.71934f		,0.707107f	,0.694658f	,0.681998f	,0.669131f	,0.656059f	,0.642788f		,/*41-50*/
	0.62932f	,0.615661f	,0.601815f		,0.587785f		,0.573576f	,0.559193f	,0.544639f	,0.529919f	,0.515038f	,0.5f			,/*51-60*/
	0.48481f	,0.469471f	,0.45399f		,0.438371f		,0.422618f	,0.406737f	,0.390731f	,0.374606f	,0.358368f	,0.34202f		,/*61-70*/
	0.325568f	,0.309017f	,0.292372f		,0.275637f		,0.258819f	,0.241922f	,0.224951f	,0.207912f	,0.190809f	,0.173648f		,/*71-80*/
    0.156434f	,0.139173f	,0.121869f		,0.104528f		,0.0871556f	,0.0697563f	,0.0523358f	,0.0348993f	,0.0174522f	,-1.73205e-07f	 /*81-90*/
};
 /**
  * @brief  基于查表法的快速余弦值计算（0-90°预存，利用三角对称性扩展）
  * @param  angle 输入角度值（整数，单位：°），支持任意整数角度（正负/超过360°）
  * @note  依赖预存的0-90°余弦值表，通过三角函数奇偶性/周期性扩展计算范围（cosθ=cos(-θ), cos(180°-θ)=-cosθ等）
  * @attention 输入非整数角度时会截断为整数，可能导致精度损失；大跨度角度（如±1e5°）会触发多次递归，影响实时性
  * @attention 输入角度最好在0~360度之间，不然容易让程序卡（每差一个周期就要循环一次）
  * @return 计算值
  * @example angle=100,返回0.866025
  */
float fastCos(int angle)
{
	if (angle>=0 && angle <= 90)
	{
		return fast_cos_buf_[angle];
	}
	else if (angle > 90 && angle <=180)
	{
		return -(fast_cos_buf_[180-angle]);
	}
	else if (angle > 180 && angle <=360)
	{
		return fastCos(360-angle);
	}
	else if (angle > 360)
	{
		return fastCos(angle - 360);
	}
	else if (angle < 0)
	{
		return (fastCos(-angle));
	}
	return 0;
}
/**
  * @brief  基于fastCos的快速正弦值计算（sinθ=cos(θ-90°)）
  * @param  angle 输入角度值（整数，单位：°），支持任意整数角度（正负/超过360°）
  * @note  本质是调用fastCos(angle-90°)，利用余弦表间接计算正弦值
  * @attention 输入非整数角度时会截断为整数，且与fastCos共享递归性能风险
  * @note 根据三角函数周期性和对称性，只需取90个已知值就能计算出所有角度值
  * @attention 输入角度最好在90~450度之间，不然容易让程序卡（每差一个周期就要循环一次）
  * @example angle=100,返回0.984808
  * @return 输出值
  */
float fastSin(int angle){
	return fastCos(angle - 90);
}
/*********大佬写的快速开平方*****************************/
/**
 * @brief  基于Quake3算法的快速平方根倒数计算（用于1/√x）
 * @param  f_num 输入被开方数（需≥0，否则行为未定义）
 * @note  使用魔数0x5f3759df的牛顿迭代法，比标准库sqrt()快约4倍，但精度略低（误差约1e-3）
 * @attention 输入负数时返回NaN（因f_num=0判断仅处理0值，负数会进入未定义位操作）
 * @attention 输入值必须大于等于0 
 * @return 输出值
 * @bug 输入值为负数时，会返回NaN
 * @example f_num=4,返回2
 */
float quakeSqrt(float f_num){
    if(f_num == 0) return 0; 
    float result = f_num; 
    float num_half = 0.5f*result; 
    int i = *(int*)&result; 

    i = 0x5f3759df - (i>>1); 
    result = *(float*)&i; 
    result = result*(1.5f-num_half*result*result);
    result = result*(1.5f-num_half*result*result); 
    return 1.0f/result; 
}
/**
 * @brief  将任意角度归一化到[0,T)区间
 * @param  Num  原始角度值(不限制单位，给定周期就好)
 * @param  T    角度周期
 * @note  基于fmod函数实现，处理逻辑：Num = Num % T → 若负则+T
 * @attention 当T非正数时会导致除零错误（需调用前确保T>0）
 * @return float            输出角度
 * @example  Num=100+360,T=360,返回100
 */
float  To_0_T(float Num,float T){
	Num=(Num?fmod(Num,T):0);
	if(Num>=0)
		return Num;
	else
		return Num+T;
}
/**
 * @brief  在角度周期性下，找到与目标角度Num_B最近的参考角度Num_A同余值
 * @param  Num_A  参考角度
 * @param  Num_B  目标角度
 * @param  T      角度周期
 * @note  通过计算周期差值dT，生成5个候选同余值（dT-2到dT+2倍周期偏移），取与Num_B绝对差最小的
 * @attention 当角度跨度超过2个周期时（如Num_A=0°, Num_B=1000°, T=360°），可能遗漏更近的候选值
 * @return float            距离B最近的A值
 * @example  Num_A=100+360*2,Num_B=50,T=360,返回100-50=50
 */
float find_min_angle(float Num_A,float Num_B,int T){//寻找距离B最近的A值
	int dT=((int)Num_B/(int)T-(int)Num_A/(int)T);//找到二者周期差值
	float meas_real[5]={Num_A+(dT-2)*T,Num_A+(dT-1)*T,Num_A+dT*T,Num_A+(dT+1)*T,Num_A+(dT+2 )*T};//找到A距离B的最小值的5种可能
	uint8_t min=0;
	for(uint8_t i=0;i<5;i++){
		if(fabs(meas_real[i]-Num_B)<fabs(meas_real[min]-Num_B))//找到所有可能中最小的
			min=i;
	}
	return meas_real[min];
}
/**
 * @brief  舵轮角度优化（根据速度方向调整角度，使转向路径最短）
 * @param  angle  原始舵轮角度（浮点数，单位：°）
 * @param  v      舵轮速度（浮点数，正负表示方向）
 * @param  T      角度周期（通常为360°）
 * @note  当速度v<0时，角度减180°（T/2）并归一化，使舵轮向反方向转更小角度；v=0时强制归零
 * @attention 强制归零逻辑可能破坏舵轮位置保持需求，需根据实际场景调整
 * @return float            输出角度
 * @example  angle=100,v=-1,t=360,返回TO_0_T(100-180)=360-80=280
 */
float  Helm_angle(float angle,float v,float T){
	if(v<0)
		angle-=T/2;
	else if(v==0)	
		angle=0;
	return To_0_T(angle,T);
}
